using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuUtilities
{
    public struct Symmetric5x5Wide
    {
        /// <summary>
        /// 矩阵的左上角为3x3挡路。
        /// </summary>
        public Symmetric3x3Wide A;
        /// <summary>
        /// 矩阵左下角的2x3挡路。
        /// </summary>
        public Matrix2x3Wide B;
        /// <summary>
        /// 矩阵右下角的2x2挡路。
        /// </summary>
        public Symmetric2x2Wide D;

        /// <summary>
        /// 按给定比例缩放m的每个分量。
        /// </summary>
        /// <param name="m">要缩放的矩阵。</param>
        /// <param name="scale">要应用于m的组件的比例。</param>
        /// <param name="result">按比例缩放m的每个组件的结果。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Scale(in Symmetric5x5Wide m, in Vector<float> scale, out Symmetric5x5Wide result)
        {
            Symmetric3x3Wide.Scale(m.A, scale, out result.A);
            Matrix2x3Wide.Scale(m.B, scale, out result.B);
            Symmetric2x2Wide.Scale(m.D, scale, out result.D);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Invert(in Symmetric3x3Wide a, in Matrix2x3Wide b, in Symmetric2x2Wide d, out Symmetric5x5Wide result)
        {
            // [A BT]^-1=[(A-BT*D^-1*B)^-1,-(A-BT*D^-1*B)^-1*BT*D^-1]
            // [B D][对称D^-1+D^-1*B*(A-BT*D^-1*B)^-1*BT*D^-1]
            Symmetric2x2Wide.InvertWithoutOverlap(d, out var invD);
            Symmetric2x2Wide.MultiplyTransposed(b, invD, out var bTInvD);
            Symmetric3x3Wide.CompleteMatrixSandwich(bTInvD, b, out var bTInvDB);
            Symmetric3x3Wide.Subtract(a, bTInvDB, out var resultAInverse);
            Symmetric3x3Wide.Invert(resultAInverse, out result.A);

            Symmetric3x3Wide.MultiplyByTransposed(result.A, bTInvD, out var negatedResultBT);
            Matrix2x3Wide.Negate(negatedResultBT, out result.B);
            Symmetric2x2Wide.CompleteMatrixSandwich(bTInvD, negatedResultBT, out result.D);
            Symmetric2x2Wide.Add(result.D, invD, out result.D);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void InvertWithoutOverlap(in Symmetric5x5Wide m, out Symmetric5x5Wide result)
        {
            Invert(m.A, m.B, m.D, out result);
        }

        /// <summary>
        /// 计算RESULT=v*m,其中v和RESULT是被分割为两个子向量的1x5向量。
        /// </summary>
        /// <param name="V0">a向量的前半部分。</param>
        /// <param name="v1">a向量的后半部分。</param>
        /// <param name="m">要转换的矩阵。</param>
        /// <param name="result t0">结果的前半部分。</param>
        /// <param name="result t1">结果的后半部分。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void TransformWithoutOverlap(in Vector3Wide v0, in Vector2Wide v1, in Symmetric5x5Wide m, out Vector3Wide result0, out Vector2Wide result1)
        {
            // [v0x v0y v0z v1x v1y]*[m.A.XX m.A.YX m.A.ZX b.X.X b.Y.X]
            // [m.A.YX m.A.YY m.A.ZY b.X.Y b.Y.Y]
            // [m.A.ZX m.A.ZY m.A.ZZ b.X.Z b.Y.Z]
            // [b.X.X b.X.Y b.X.Z d.XX d.YX]
            // [b.Y.X b.Y.Y b.Y.Z d.YX d.YY]
            result0.X = v0.X * m.A.XX + v0.Y * m.A.YX + v0.Z * m.A.ZX + v1.X * m.B.X.X + v1.Y * m.B.Y.X;
            result0.Y = v0.X * m.A.YX + v0.Y * m.A.YY + v0.Z * m.A.ZY + v1.X * m.B.X.Y + v1.Y * m.B.Y.Y;
            result0.Z = v0.X * m.A.ZX + v0.Y * m.A.ZY + v0.Z * m.A.ZZ + v1.X * m.B.X.Z + v1.Y * m.B.Y.Z;

            result1.X = v0.X * m.B.X.X + v0.Y * m.B.X.Y + v0.Z * m.B.X.Z + v1.X * m.D.XX + v1.Y * m.D.YX;
            result1.Y = v0.X * m.B.Y.X + v0.Y * m.B.Y.Y + v0.Z * m.B.Y.Z + v1.X * m.D.YX + v1.Y * m.D.YY;
        }
    }
}
